#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <memory.h>
#include <string.h>
#include <unistd.h>
#include "iot_watchdog.h"
#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_pwm.h"
#include "hi_pwm.h"
#include "iot_gpio_ex.h"
#include "hi_timer.h"
#include "robot_motor.h"
#include "robot_encoder.h"
#include "robot_sg90.h"
#include "robot_hcsr04.h"

//#define DEBUG

#define GPIO5 5
#define FUNC_GPIO 0
#define IOT_IO_PULL_UP 1
#define ONE_SECOND (1000)

#define MODE_NONE 0
#define MODE_TRACKING 170         // 跟随模式
#define MODE_IDENTIFY 255         // 识别模式

float obstacle_threshold = 30;           // 障碍物有效半径
float histogram[7];
extern hi_bool Follow_Switch;
extern hi_bool Obs_Signal;
extern int Detect_Mode;

void ObstacleDetectionTaskEntry(void){
    // 栅格图赋初值 对应角度 -90 -60 -30 0 30 60 90
    for (int i=0;i<7;i++){
        histogram[i] = 0;
    }

    printf("[LOG] Obstacle detection initialized...\r\n");
    
    while(1){
        if (Follow_Switch && Detect_Mode != MODE_IDENTIFY){
            // 障碍物距离(单位cm)
            float left_distance = 0;
            float right_distance = 0;
            float forward_distance = 0;
            unsigned int time = 100;
            
            // 舵机往左转动测量左边障碍物的距离
            engine_turn_left();
            hi_sleep(time);
            left_distance = GetDistance();
            hi_sleep(time);

            // 归中测量前方障碍物
            regress_middle();
            hi_sleep(time);
            forward_distance = GetDistance();
            hi_sleep(time);

            // 舵机往右转动测量右边障碍物的距离
            engine_turn_right();
            hi_sleep(time);
            right_distance = GetDistance();
            hi_sleep(time);

            // 归中
            regress_middle();

#ifdef DEBUG
            printf("[DEBUG] Obstacle distance left %.2f forward %.2f right %.2f\r\n", left_distance, forward_distance, right_distance);
#endif

            if (Detect_Mode != MODE_IDENTIFY){
                // 判断是否在有效范围内
                left_distance = (left_distance < obstacle_threshold)? obstacle_threshold - left_distance: 0;
                right_distance = (right_distance < obstacle_threshold)? obstacle_threshold - right_distance: 0;
                forward_distance = (forward_distance < obstacle_threshold*0.5)? obstacle_threshold - forward_distance: 0;

                histogram[0] = left_distance + 0.25*forward_distance;                           // -90 degree
                histogram[1] = 0.75*left_distance + 0.5*forward_distance;                       // -60 degree
                histogram[2] = 0.5*left_distance + 0.75*forward_distance;                       // -30 degree
                histogram[3] = forward_distance + 0.25*right_distance + 0.25*left_distance;     // 0 degree
                histogram[4] = 0.5*right_distance + 0.75*forward_distance;                      // 30 degree
                histogram[5] = 0.75*right_distance + 0.5*forward_distance;                      // 60 degree
                histogram[6] = right_distance + 0.25*forward_distance;                          // 90 degree

#ifdef DEBUG
                printf("[VFH]");
                for(int i=0;i<7;i++) printf("%.2f ",histogram[i]);
                printf("\r\n");                
#endif

                Obs_Signal = HI_FALSE;

                for (int i=0;i<7;i++){
                    if (histogram[i] > 0){
                        Obs_Signal = HI_TRUE;
                        break;
                    }
                }

                if (Obs_Signal && Detect_Mode != MODE_IDENTIFY){
                    float min_loss = 1000;
                    int min_index = 0;
                    float Kobs = 10;
                    float Ktheta = 0.5;
#ifdef DEBUG
                printf("[VFH]");                
#endif
                    for (int i=0;i<7;i++){
                        float loss = Kobs*histogram[i] + Ktheta*fabs((-90+i*30));
                        if (loss < min_loss){
                            min_loss = loss;
                            min_index = i;
                        }
#ifdef DEBUG
                    printf("%.2f ",loss);             
#endif
                    }
#ifdef DEBUG
                printf("\r\n");                
#endif                 
                    int direction = -90+min_index*30; 
#ifdef DEBUG
                printf("[DEBUG] minloss %2.f direction %d\r\n", min_loss, direction);                
#endif

                    int offset = 0;    // 额外偏置

                    if (direction > 0){
                        car_turnRight(fabs(direction));
                        car_forward(0.3);
                        car_turnLeft(fabs(direction)+offset);
                    }
                    else {
                        car_turnLeft(fabs(direction));-                                                                
                        car_forward(0.3);
                        car_turnRight(fabs(direction)+offset);
                    }
                }
            }
        }
        TaskMsleep(1*ONE_SECOND);
    }
}

#define     OBSTACLE_DETECTION_TASK_STAK_SIZE   (1024*10)
#define     OBSTACLE_DETECTION_TASK_PRIORITY    (29)

/* 避障检测线程初始化 */
void ObstacleDetectionTask(void)
{
    osThreadAttr_t attr_obs;

    attr_obs.name = "ObstacleAvoidanceTask";


    attr_obs.attr_bits = 0U;
    attr_obs.cb_mem = NULL;
    attr_obs.cb_size = 0U;
    attr_obs.stack_mem = NULL;
    attr_obs.stack_size = OBSTACLE_DETECTION_TASK_STAK_SIZE;
    attr_obs.priority = OBSTACLE_DETECTION_TASK_PRIORITY;

    if (osThreadNew(ObstacleDetectionTaskEntry, NULL, &attr_obs) == NULL) {
        printf("[Ssd1306TestDemo] Falied to create RobotCarTestTask!\n");
    }
}

APP_FEATURE_INIT(ObstacleDetectionTask);